#ifndef XV_HAMILTON_QUAT_OPS_H
#define XV_HAMILTON_QUAT_OPS_H

#include <Eigen/Core>
#include <Eigen/Geometry>
#include "QuatOps.h"

namespace xvslam {

class HamiltonQuatOps {

public:
    // 存储顺序同步Eigen， x, y, z, w
    static inline Eigen::Matrix<double, 4, 1> rot_2_quat(const Eigen::Matrix<double, 3, 3> &rot){
        Eigen::Matrix<double, 4, 1> q;
        double T = rot.trace();
        if (T > 0.0f) {
            double s = 0.5 / sqrt(T + 1.0);
            q(3) = 0.25 / s;
            q(0) = (rot(2,1) - rot(1,2)) * s;
            q(1) = (rot(0,2) - rot(2,0)) * s;
            q(2) = (rot(1,0) - rot(0,1)) * s;
        } else {
            size_t i = 0;
            if (rot(1, 1) > rot(0, 0)) {
                i = 1;
            }
            if (rot(2, 2) > rot(i, i)){
                i = 2;
            }
            size_t j = (i + 1) % 3;
            size_t k = (j + 1) % 3;
            double s = sqrt(rot(i, i) - rot(j, j) - rot(k, k) + 1.0f);
            q(i) = 0.5 * s;
            s = 0.5 / s;
            q(3) = (rot(k,j) - rot(j,k)) * s;
            q(j) = (rot(j,i) + rot(i,j)) * s;
            q(k) = (rot(k,i) + rot(i,k)) * s;
        }
        if (q(3) < 0) {
            q = -q;
        }
        q = q / (q.norm());
        return q;
    }

    // 按 x y z w 来写
    static inline Eigen::Matrix<double, 3, 3> quat_2_rot(const Eigen::Matrix<double, 4, 1> &q) {
        Eigen::Matrix<double, 3, 3> rot;
        double tx = q(0);
        double ty = q(1);
        double tz = q(2);
        double tw = q(3);
        double twx = 2.0f * tw * tx;
        double twy = 2.0f * tw * ty;
        double twz = 2.0f * tw * tz;
        double txx = 2.0f * tx * tx;
        double txy = 2.0f * tx * ty;
        double txz = 2.0f * tx * tz;
        double tyy = 2.0f * ty * ty;
        double tyz = 2.0f * ty * tz;
        double tzz = 2.0f * tz * tz;
        rot(0,0) = 1.0f - (tyy + tzz);
        rot(0,1) = txy - twz;
        rot(0,2) = txz + twy;
        rot(1,0) = txy + twz;
        rot(1,1) = 1.0f - (txx + tzz);
        rot(1,2) = tyz - twx;
        rot(2,0) = txz - twy;
        rot(2,1) = tyz + twx;
        rot(2,2) = 1.0f - (txx + tyy);
        return rot;
    }

    static inline Eigen::Matrix<double, 4, 1> quat_multiply(const Eigen::Matrix<double, 4, 1> &q, const Eigen::Matrix<double, 4, 1> &p) {
        Eigen::Matrix<double, 4, 1> q_t;
        double p_w = p(3, 0);
        double q_w = q(3, 0);
        Eigen::Matrix<double, 3, 1> p_vec = p.block(0, 0, 3, 1);
        Eigen::Matrix<double, 3, 1> q_vec = q.block(0, 0, 3, 1);
        q_t(3, 0) = q_w * p_w - q_vec.dot(p_vec);
        q_t.block(0, 0, 3, 1) = q_w * p_vec + p_w * q_vec + q_vec.cross(p_vec);
        if (q_t(3) < 0) {
            q_t *= -1;
        }
        return q_t / q_t.norm();
    }
        
    static inline Eigen::Matrix<double, 4, 1> Inv(Eigen::Matrix<double, 4, 1> q) {
        Eigen::Matrix<double, 4, 1> qinv;
        double tx = q(0);
        double ty = q(1);
        double tz = q(2);
        double tw = q(3);
        auto norm = 1.0 / (tx * tx + ty * ty + tz * tz + tw * tw);
        qinv(0) = tx * norm;
        qinv(1) = ty * norm;
        qinv(2) = tz * norm;
        qinv(3) = tw * norm;
        return qinv;
    }

    static inline Eigen::Matrix<double, 4, 1> quatnorm(Eigen::Matrix<double, 4, 1> q_t) {
        if (q_t(3, 0) < 0) {
            q_t *= -1;
        }
        return q_t / q_t.norm();
    }

private:
    HamiltonQuatOps() {}
    
    Eigen::Matrix<double, 3, 3> quat_2_rot_equation(const Eigen::Matrix<double, 4, 1> &q) {
        Eigen::Matrix<double, 3, 3> q_x = skew_x(q.block(0, 0, 3, 1));
        Eigen::MatrixXd rot = q.block(0, 0, 3, 1) * (q.block(0, 0, 3, 1).transpose()) + 
            std::pow(q(3, 0), 2) * Eigen::MatrixXd::Identity(3, 3) + 2 * q(3, 0) * q_x + q_x * q_x;
        return rot;
    }

    inline Eigen::Matrix<double, 4, 1> conjugate(const Eigen::Matrix<double, 4, 1> &q) {
        Eigen::Matrix<double, 4, 1> qcon;
        qcon.block(0, 0, 3, 1) = -q.block(0, 0, 3, 1);
        return qcon;
    }
};

}

#endif